#include "uart.h"
#include <stdio.h>
using namespace std;

UART::UART():Serial()
{
    ros::NodeHandle nh;
    nh.param<std::string>("Port",_port,"/dev/ttyUSB0");
    nh.param<int>("Baudrate",_baudrate,115200);
    _to = Timeout::simpleTimeout(100);

}

void UART::state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}



void UART::odometry_callback(const nav_msgs::Odometry &current_info)
{
    // current_altitude = current_info.pose.pose.position.z*100;

    current_angle.x= current_info.pose.pose.orientation.x;
    current_angle.y= current_info.pose.pose.orientation.y;
    current_angle.z= current_info.pose.pose.orientation.z;
    current_angle.w= current_info.pose.pose.orientation.w;
    curr_angle = toEulerAngle(current_angle);
}


void UART::velocity_callback(const geometry_msgs::TwistStamped &msg)
{
    current_vx = msg.twist.linear.x;
    current_vy = msg.twist.linear.y;
}





void UART::angle_cb(const std_msgs::Float32MultiArray &msg)
{
    
    Int2char2 pitch_angle_union, yaw_angle_union;
    pitch_angle_union.Int = -msg.data[1];
    yaw_angle_union.Int = -msg.data[0];

    uint8_t  temp1[18] = {0x3e,0x43,0x0d,0x50,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01};
    
    temp1[5] = pitch_angle_union.Char[0];
    temp1[6] = pitch_angle_union.Char[1];

    temp1[13] = yaw_angle_union.Char[0];
    temp1[14] = yaw_angle_union.Char[1];

    temp1[17] = temp1[4]+temp1[5]+temp1[6]+temp1[7]+temp1[8]+temp1[9]+temp1[10]+temp1[11]+temp1[12]+temp1[13]+temp1[14]+temp1[15]+temp1[16];

    write(temp1, 18);
    std::cout<<"angle:"<<-msg.data[0]<<", "<<-msg.data[1]<<std::endl;
    // std::cout<<"publish once .... "<<hex<<static_cast<unsigned int>(temp1[13])<<", "<<static_cast<unsigned int>(temp1[14])<<std::endl;

}


void UART::OpenSerial()
{
    try {
        this->open();
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM("Unable to open port.");
        return;
    }

    if(this->isOpen())
    {
        ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
    }
    else
    {
        return;
    }
}

uint8_t UART::CrcCheck(std::vector<uint8_t> buff,int len)
{
    uint8_t crc = 0;

    for(uint8_t i=0;i<len;i++)
    {
        crc += buff[i];
    }

    return crc;
}

geometry_msgs::Vector3 UART::toEulerAngle(geometry_msgs::Quaternion quat)
{
    geometry_msgs::Vector3 ans;

    tf::Matrix3x3 R_FLU2ENU(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
    R_FLU2ENU.getRPY(ans.x, ans.y, ans.z);
    return ans;
}


void UART::RunMain()
{   
    
    uint8_t package[256];

    uint8_t rx_head = 0;
    uint8_t rx_wr_index = 0;
    uint8_t b_rx_over = 0;
    serial_process_t _serial_process;
    OpenSerial();
    ros::Rate loop_rate(60);

    ros::NodeHandle nh2;
    //advertise是接收串口数据，发送到ros
    //subscribe是接收ros数据，发送到串口
    // ros::Publisher _global_pub2 = nh2.advertise<sensor_msgs::NavSatFix>("/fromothermaster/global",10);
    // ros::Publisher _global_pub3 = nh2.advertise<sensor_msgs::NavSatFix>("/fromotherfollower/global",10);
    ros::Subscriber _local_pos_sub2 = nh2.subscribe("/mavros/local_position/odom",10,&UART::odometry_callback,this);
    ros::Subscriber _local_vel_sub2 = nh2.subscribe("/mavros/local_position/velocity_local",10,&UART::velocity_callback,this);
    ros::Subscriber _state_sub = nh2.subscribe("/mavros/state", 10, &UART::state_cb ,this);

    ros::Subscriber _gimble_angle_sub = nh2.subscribe("/destination_servo_angle", 10, &UART::angle_cb ,this);
    

    while(ros::ok())
    {

        // //read serial
        // size_t n = this->available();

        // uint8_t buffer[256];
        // n = this->read(buffer,n);//must! n<=256
        // uint8_t b[256];
        // for(uint8_t i=0;i<n;i++)
        // {
        //     b[i] = buffer[i];
        // }

        // for(uint8_t i=0;i<n;i++)
        // {
        //     if(b[i]==0xb5)
        //     {
        //         rx_head = 1;
        //         package[rx_wr_index++] = b[i];
        //     }
        //     else if(b[i]==0x62)
        //     {
        //         if(rx_head)
        //         {
        //             rx_wr_index = 0;
        //             b_rx_over = 0;
        //         }
        //         else
        //         {
        //             package[rx_wr_index++] = b[i];
        //             rx_head = 0;
        //         }

        //     }
        //     else
        //     {
        //         package[rx_wr_index++] = b[i];
        //         rx_head = 0;
        //         if(rx_wr_index>32) //>4*8 zanding
        //         {
        //             if((rx_wr_index+2) == package[3] )
        //             {

        //                 std::cout<<"get the whole package."<<std::endl;
        //                 b_rx_over = 1;

        //                 _serial_process.rx_buffer.clear();
        //                 _serial_process.rx_buffer.push_back(0xb5);
        //                 _serial_process.rx_buffer.push_back(0x62);

        //                 for(uint8_t i=2; i<package[3];i++)
        //                 {
        //                     _serial_process.rx_buffer.push_back(package[i-2]);
        //                 }
        //                 _serial_process.index = package[3];
        //                 _serial_process.rx_over = 1;
        //                 memset(package,0,256);
        //             }
        //         }
        //     }
        //     if(rx_wr_index>200)
        //         rx_wr_index=200;
        // }

        // if(_serial_process.rx_over==1)
        // {
        //     std::vector<uint8_t> buffer;
        //     for(uint8_t i=0;i<_serial_process.index;i++)
        //     {

        //         buffer.push_back(_serial_process.rx_buffer[i]);
        //     }
        //     // std::cout<<(int)buffer[0]<<std::endl;
        //     if(buffer[0]==0xb5 && buffer[1]==0x62 && buffer[_serial_process.index-2]==0x0d && buffer[_serial_process.index-1]==0x0a)
        //     {
        //         std::cout<<"check header successfully!"<<std::endl;
        //         //crc check
        //         uint8_t crc = CrcCheck(buffer,_serial_process.index-3);
        //         if(crc == buffer[_serial_process.index-3])
        //         {
        //             uint8_t temp[_serial_process.index];
        //             for(uint8_t i=0;i<_serial_process.index;i++)
        //             {
        //                 temp[i] = buffer[i];
        //             }
        //             std::cout<<"check crc successfully!"<<std::endl;

        //             // memcpy(&_gpsRec,temp+10,25);
        //             // //publish gps
        //             // sensor_msgs::NavSatFix gps_pos;
        //             // gps_pos.position_covariance[0] = temp[9];//_gpsRec.vehicleid;
        //             // gps_pos.latitude = _gpsRec.latitude/10000000.0;
        //             // gps_pos.longitude = _gpsRec.longitude/10000000.0;
        //             // gps_pos.altitude = _gpsRec.altitude/100.0;
        //             // gps_pos.position_covariance[1] = _gpsRec.yaw/100.0;
        //             // //position_covariance[0]->master or follower , position_covariance[1]->vehicle id
        //             // gps_pos.position_covariance[2] = _gpsRec.command;

                    
        //             // if(gps_pos.position_covariance[2] ==1)
        //             // {
        //             //     _global_pub2.publish(gps_pos);
        //             //     std::cout<<"receive message from other master !"<<std::endl;
        //             // }
        //             // else if(gps_pos.position_covariance[2] ==2)
        //             // {
        //             //     _global_pub3.publish(gps_pos);
        //             //     std::cout<<"receive message from other followers !"<<std::endl;
        //             // }

        //         }
        //     }

        //     _serial_process.rx_over = 0;
        // }


        ros::spinOnce();
        loop_rate.sleep();
    }
}
